/*=========================================================================
	UberSim Source Code Release
	-------------------------------------------------------------------------
	Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
	James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
	Erick Tryzelaar {erickt}@andrew.cmu.edu
	School of Computer Science, Carnegie Mellon University
	-------------------------------------------------------------------------
	This software is distributed under the GNU General Public License,
	version 2.  If you do not have a copy of this licence, visit
	www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
	Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
	in the hope that it will be useful, but WITHOUT ANY WARRANTY,
	including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
	-------------------------------------------------------------------------*/

#include <ode/ode.h>
#include <typeinfo>

#include "../utils/configreader.h"

#include "mmgr.h"

#include "BoxGeometry.h"
#include "CappedCylinderGeometry.h"
#include "CylinderGeometry.h"
#include "OmniRobot.h"
#include "Event.h"
#include "Geometry.h"
#include "Hinge2Joint.h"
#include "Node.h"
#include "RigidBody.h"
#include "RigidBodyCollider.h"
#include "SliderJoint.h"
#include "SphereGeometry.h"
#include "Surface.h"
#include "Timer.h"

CR_DECLARE (MASS);
CR_DECLARE (SHELL_LENGTH);
CR_DECLARE (SHELL_WIDTH);
CR_DECLARE (SHELL_HEIGHT);
CR_DECLARE (SHELL_POSITION);
CR_DECLARE (WHEEL_LENGTH);
CR_DECLARE (WHEEL_RADIUS);
CR_DECLARE (WHEEL_MU);
CR_DECLARE (WHEEL_MU2);
CR_DECLARE (WHEEL_SLIP1);
CR_DECLARE (WHEEL_SLIP2);
CR_DECLARE (WHEEL_MAX_FORCE);
CR_DECLARE (WHEEL_MASS);
CR_DECLARE (WHEEL1_POSITION);
CR_DECLARE (WHEEL2_POSITION);
CR_DECLARE (WHEEL3_POSITION);
CR_DECLARE (STOPPER_LENGTH);
CR_DECLARE (STOPPER_WIDTH);
CR_DECLARE (STOPPER_HEIGHT);
CR_DECLARE (LEFT_STOPPER_POSITION);
CR_DECLARE (RIGHT_STOPPER_POSITION);
CR_DECLARE (DRIBBLER_LENGTH);
CR_DECLARE (DRIBBLER_RADIUS);
CR_DECLARE (DRIBBLER_MU);
CR_DECLARE (DRIBBLER_SLIP1);
CR_DECLARE (DRIBBLER_SLIP2);
CR_DECLARE (DRIBBLER_MAX_FORCE);
CR_DECLARE (DRIBBLER_TARGET_VELOCITY);
CR_DECLARE (DRIBBLER_MASS);
CR_DECLARE (DRIBBLER_POSITION);
CR_DECLARE (KICKER_LENGTH);
CR_DECLARE (KICKER_WIDTH);
CR_DECLARE (KICKER_HEIGHT);
CR_DECLARE (KICKER_MAX_FORCE);
CR_DECLARE (KICKER_LOWER_LIMIT);
CR_DECLARE (KICKER_UPPER_LIMIT);
CR_DECLARE (KICKER_TARGET_VELOCITY);
CR_DECLARE (KICKER_MASS);
CR_DECLARE (KICKER_POSITION);
CR_DECLARE (KICKER_DELAY);

OmniRobot::OmniRobot (const Vector3& color)
{
	static bool isConfiged = false;

	if (!isConfiged)
	{
		isConfiged = true;

		CR_SETUP (OmniRobot, MASS, CR_DOUBLE);
		CR_SETUP (OmniRobot, SHELL_LENGTH, CR_DOUBLE);
		CR_SETUP (OmniRobot, SHELL_WIDTH, CR_DOUBLE);
		CR_SETUP (OmniRobot, SHELL_HEIGHT, CR_DOUBLE);
		CR_SETUP (OmniRobot, SHELL_POSITION, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL_LENGTH, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL_RADIUS, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL_MU, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL_MU2, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL_SLIP1, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL_SLIP2, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL_MAX_FORCE, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL_MASS, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL1_POSITION, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL2_POSITION, CR_DOUBLE);
		CR_SETUP (OmniRobot, WHEEL3_POSITION, CR_DOUBLE);
		CR_SETUP (OmniRobot, STOPPER_LENGTH, CR_DOUBLE);
		CR_SETUP (OmniRobot, STOPPER_WIDTH, CR_DOUBLE);
		CR_SETUP (OmniRobot, STOPPER_HEIGHT, CR_DOUBLE);
		CR_SETUP (OmniRobot, LEFT_STOPPER_POSITION, CR_DOUBLE);
		CR_SETUP (OmniRobot, RIGHT_STOPPER_POSITION, CR_DOUBLE);
		CR_SETUP (OmniRobot, DRIBBLER_LENGTH, CR_DOUBLE);
		CR_SETUP (OmniRobot, DRIBBLER_RADIUS, CR_DOUBLE);
		CR_SETUP (OmniRobot, DRIBBLER_MU, CR_DOUBLE);
		CR_SETUP (OmniRobot, DRIBBLER_SLIP1, CR_DOUBLE);
		CR_SETUP (OmniRobot, DRIBBLER_SLIP2, CR_DOUBLE);
		CR_SETUP (OmniRobot, DRIBBLER_MAX_FORCE, CR_DOUBLE);
		CR_SETUP (OmniRobot, DRIBBLER_TARGET_VELOCITY, CR_DOUBLE);
		CR_SETUP (OmniRobot, DRIBBLER_MASS, CR_DOUBLE);
		CR_SETUP (OmniRobot, DRIBBLER_POSITION, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_LENGTH, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_WIDTH, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_HEIGHT, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_MAX_FORCE, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_LOWER_LIMIT, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_UPPER_LIMIT, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_TARGET_VELOCITY, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_MASS, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_POSITION, CR_DOUBLE);
		CR_SETUP (OmniRobot, KICKER_DELAY, CR_DOUBLE);
	}

	Real length = DVAR (SHELL_LENGTH);
	Real width  = DVAR (SHELL_WIDTH);
	Real height = DVAR (SHELL_HEIGHT);

	////

	Node* robot = new Node ("Omni Robot");
	this->model = robot;

	rigidBody        = new RigidBody ();
	motionController = rigidBody;
	rigidBody->setModel (robot);

	dMass bodyMass;
	dMassSetBox  (&bodyMass, 1, length, width, height);
	dMassAdjust  (&bodyMass, DVAR (MASS));
	dBodySetMass (rigidBody->getBody (), &bodyMass);

	////

	Geometry* shellGeometry = new BoxGeometry (length, width, height, "Shell");
	shellGeometry->setColor (color);
	robot->addChild                       (shellGeometry);
	shellGeometry->setCollisionController (new RigidBodyCollider (rigidBody));
	shellGeometry->setLocalPosition       (Vector3 (VDVAR (SHELL_POSITION)));

	////

	Node* wheels = new Node ("Wheels");
	robot->addChild (wheels);

	////

	Geometry* wheel1Geometry = new CappedCylinderGeometry (DVAR (WHEEL_LENGTH), DVAR (WHEEL_RADIUS), "Wheel 1");
	wheels->addChild (wheel1Geometry);

	wheel1Body = new RigidBody ();
	wheel1Body->setModel        (wheel1Geometry);	
	wheel1Body->setLocalRotation (Matrix33::rotateAboutX (Math::HALF_PI) * Matrix33::rotateAboutY (Math::PI / 6));
	//wheel1Body->setLocalRotation (Matrix33::rotateAboutX (Math::HALF_PI));// * Matrix33::rotateAboutY (Math::PI / 3));
	wheel1Body->setWorldPosition (Vector3 (VDVAR (WHEEL1_POSITION)));

	//dBodySetFiniteRotationMode (wheel1Body->getBody (), 1);
	
	Surface* wheel1Surface = new Surface ();
	dSurfaceParameters* sp = &wheel1Surface->getSurfaceParameters ();
	sp->mode  = dContactApprox1 | dContactFDir1 | dContactMu2;// | dContactSlip1 | dContactSlip2;
	sp->mu    = DVAR (WHEEL_MU);
	sp->mu2   = DVAR (WHEEL_MU2);
	sp->slip1 = DVAR (WHEEL_SLIP1);
	sp->slip2 = DVAR (WHEEL_SLIP2);

	wheel1Collider = new RigidBodyCollider (wheel1Body);
	wheel1Collider->setSurface (wheel1Surface);
	wheel1Geometry->setCollisionController (wheel1Collider);

	wheel1Motor = new Hinge2Joint (rigidBody, wheel1Body);
	wheel1Motor->setAnchor (wheel1Body->getWorldPosition ());
	wheel1Motor->setAxis1     (Vector3 (0.0,  0.0, 1.0));
	//wheel1Motor->setAxis2     (Vector3 (1.0, -1.0, 0.0));  
	wheel1Motor->setAxis2     (Vector3 (VDVAR (WHEEL1_POSITION)[0], VDVAR (WHEEL1_POSITION)[1], 0.0).unit ());
	wheel1Motor->setMaxForce2 (DVAR (WHEEL_MAX_FORCE));

	dMass wheel1Mass;
	dMassSetSphere (&wheel1Mass, 1, DVAR (WHEEL_RADIUS));
	dMassAdjust    (&wheel1Mass, DVAR (WHEEL_MASS));
	dBodySetMass   (wheel1Body->getBody (), &wheel1Mass);

	////

	Geometry* wheel2Geometry = new CappedCylinderGeometry (DVAR (WHEEL_LENGTH), DVAR (WHEEL_RADIUS), "Wheel 2");
	wheels->addChild (wheel2Geometry);

	wheel2Body = new RigidBody ();
	wheel2Body->setModel (wheel2Geometry);
	wheel2Body->setLocalRotation (Matrix33::rotateAboutX (Math::HALF_PI) * Matrix33::rotateAboutY (Math::PI / 2));
	wheel2Body->setWorldPosition (Vector3 (VDVAR (WHEEL2_POSITION)));

	//dBodySetFiniteRotationMode (wheel2Body->getBody (), 1);
	
	Surface* wheel2Surface = new Surface ();
	sp = &wheel2Surface->getSurfaceParameters ();
	sp->mode  = dContactApprox1 | dContactFDir1 | dContactMu2;// | dContactSlip1 | dContactSlip2;
	sp->mu    = DVAR (WHEEL_MU);
	sp->mu2   = DVAR (WHEEL_MU2);
	sp->slip1 = DVAR (WHEEL_SLIP1);
	sp->slip2 = DVAR (WHEEL_SLIP2);
/*
	sp->mu    = 0.0;
	sp->mu2   = 0.0;
	sp->slip1 = 0.0;
	sp->slip2 = 0.0;
*/
	
	wheel2Collider = new RigidBodyCollider (wheel2Body);
	wheel2Collider->setSurface (wheel2Surface);
	wheel2Geometry->setCollisionController (wheel2Collider);

	wheel2Motor = new Hinge2Joint (rigidBody, wheel2Body);
	wheel2Motor->setAnchor    (wheel2Body->getWorldPosition ());
	wheel2Motor->setAxis1     (Vector3 ( 0.0, 0.0, 1.0));
	//wheel2Motor->setAxis2     (Vector3 (-1.0, 0.0, 0.0));  
	wheel2Motor->setAxis2     (Vector3 (VDVAR (WHEEL2_POSITION)[0], VDVAR (WHEEL2_POSITION)[1], 0.0).unit ());
	wheel2Motor->setMaxForce2 (DVAR (WHEEL_MAX_FORCE));

	dMass wheel2Mass;
	dMassSetSphere (&wheel2Mass, 1, DVAR (WHEEL_RADIUS));
	dMassAdjust    (&wheel2Mass, DVAR (WHEEL_MASS));
	dBodySetMass   (wheel2Body->getBody (), &wheel2Mass);

	////

	Geometry* wheel3Geometry = new CappedCylinderGeometry (DVAR (WHEEL_LENGTH), DVAR (WHEEL_RADIUS), "Wheel 3");
	wheels->addChild (wheel3Geometry);

	wheel3Body = new RigidBody ();
	wheel3Body->setModel        (wheel3Geometry);
	wheel3Body->setLocalRotation (Matrix33::rotateAboutX (Math::HALF_PI) * Matrix33::rotateAboutY (-Math::PI / 6));
	//wheel3Body->setLocalRotation (Matrix33::rotateAboutX (Math::HALF_PI));// * Matrix33::rotateAboutY (-Math::PI /3 ));
	wheel3Body->setWorldPosition (Vector3 (VDVAR (WHEEL3_POSITION)));

	//dBodySetFiniteRotationMode (wheel3Body->getBody (), 1);
	
	Surface* wheel3Surface = new Surface ();
	sp = &wheel3Surface->getSurfaceParameters ();
	sp->mode  = dContactApprox1 | dContactFDir1 | dContactMu2; //| dContactSlip1 | dContactSlip2;
	sp->mu    = DVAR (WHEEL_MU);
	sp->mu2   = DVAR (WHEEL_MU2);
	sp->slip1 = DVAR (WHEEL_SLIP1);
	sp->slip2 = DVAR (WHEEL_SLIP2);

	wheel3Collider = new RigidBodyCollider (wheel3Body);
	wheel3Collider->setSurface             (wheel3Surface);
	wheel3Geometry->setCollisionController (wheel3Collider);

	wheel3Motor = new Hinge2Joint (rigidBody, wheel3Body);
	wheel3Motor->setAnchor    (wheel3Body->getWorldPosition ());
	wheel3Motor->setAxis1     (Vector3 (0.0, 0.0, 1.0));
	//wheel3Motor->setAxis2     (Vector3 (1.0, 1.0, 0.0));
	wheel3Motor->setAxis2     (Vector3 (VDVAR (WHEEL3_POSITION)[0], VDVAR (WHEEL3_POSITION)[1], 0.0).unit ());
	wheel3Motor->setMaxForce2 (DVAR (WHEEL_MAX_FORCE));

	dMass wheel3Mass;
	dMassSetSphere (&wheel3Mass, 1, DVAR (WHEEL_RADIUS));
	dMassAdjust    (&wheel3Mass, DVAR (WHEEL_MASS));
	dBodySetMass   (wheel3Body->getBody (), &wheel3Mass);

	////
	
	Real R = wheel1Geometry->getLocalPosition ().length ();
	Real r = DVAR (WHEEL_RADIUS);


	// linear transform from robot velocities into wheel velocities
	transform = Matrix33 (
			 1.0 / (r * Math::sqrt (3.0)),  6.66 / 3.0, 2 * Math::PI * R,
			                          0.0, -2.0 * 6.66 / 3.0, 2 * Math::PI * R,
			-1.0 / (r * Math::sqrt (3.0)),  6.66 / 3.0, 2 * Math::PI * R);
/*
	transform = Matrix33 (
			 1.0 / (r * Math::sqrt (3.0)),  1.0 / 3.0, R,
			                          0.0, -2.0 / 3.0, R,
			-1.0 / (r * Math::sqrt (3.0)),  1.0 / 3.0, R);
*/
	////

	Node* dribblerBar = new Node ("Dribbler Bar");
	robot->addChild (dribblerBar);

	////

	Geometry* leftStopperGeometry = new BoxGeometry (DVAR (STOPPER_LENGTH), DVAR (STOPPER_WIDTH), DVAR (STOPPER_HEIGHT), "Left Stopper");
	dribblerBar->addChild (leftStopperGeometry);
	leftStopperGeometry->setCollisionController (new RigidBodyCollider (rigidBody));

	leftStopperGeometry->setLocalPosition (Vector3 (VDVAR (LEFT_STOPPER_POSITION)));

	////

	Geometry* rightStopperGeometry = new BoxGeometry (DVAR (STOPPER_LENGTH), DVAR (STOPPER_WIDTH), DVAR (STOPPER_HEIGHT), "Right Stopper");
	dribblerBar->addChild (rightStopperGeometry);
	rightStopperGeometry->setCollisionController (new RigidBodyCollider (rigidBody));

	rightStopperGeometry->setLocalPosition (Vector3 (VDVAR (RIGHT_STOPPER_POSITION)));

	////

	Geometry* dribblerGeometry = new CappedCylinderGeometry (DVAR (DRIBBLER_LENGTH), DVAR (DRIBBLER_RADIUS), "Dribbler");
	dribblerBar->addChild (dribblerGeometry);

	RigidBody* dribblerBody = new RigidBody ();
	dribblerBody->setModel        (dribblerGeometry);
	dribblerBody->setLocalRotation (Matrix33::rotateAboutX (Math::HALF_PI));
	dribblerBody->setWorldPosition (Vector3 (VDVAR (DRIBBLER_POSITION)));

	Surface* dribblerSurface = new Surface ();
	sp = &dribblerSurface->getSurfaceParameters ();
	//sp->mode  = dContactSlip1 | dContactSlip2;// | dContactApprox1;
	sp->mu    = DVAR (DRIBBLER_MU);
	//sp->slip1 = DVAR (DRIBBLER_SLIP1);
	//sp->slip2 = DVAR (DRIBBLER_SLIP2);

	RigidBodyCollider* dribblerCollider = new RigidBodyCollider (dribblerBody);
	dribblerCollider->setSurface (dribblerSurface);
	dribblerGeometry->setCollisionController (dribblerCollider);

	dribblerMotor = new Hinge2Joint (rigidBody, dribblerBody);
	dribblerMotor->setAnchor    (dribblerBody->getWorldPosition ());
	dribblerMotor->setAxis1     (Vector3 (0.0, 0.0, 1.0));
	dribblerMotor->setAxis2     (Vector3 (0.0, 1.0, 0.0));
	dribblerMotor->setMaxForce2 (DVAR (DRIBBLER_MAX_FORCE));

	dMass dribblerMass;
	dMassSetSphere (&dribblerMass, 1, DVAR (DRIBBLER_RADIUS));
	dMassAdjust    (&dribblerMass, DVAR (DRIBBLER_MASS));
	dBodySetMass   (dribblerBody->getBody (), &dribblerMass);

	////

	Geometry* kickerGeometry = new BoxGeometry (DVAR (KICKER_LENGTH), DVAR (KICKER_WIDTH), DVAR (KICKER_HEIGHT), "Kicker");
	robot->addChild (kickerGeometry);
	RigidBody* kickerBody = new RigidBody ();
	kickerBody->setModel (kickerGeometry);
	kickerBody->setWorldPosition (Vector3 (VDVAR (KICKER_POSITION)));

	/*
		 Surface* dribblerSurface = new Surface ();
		 sp = &dribblerSurface->getSurfaceParameters ();
		 sp->mu = Math::INFINITY;		
		 */

	RigidBodyCollider* kickerCollider = new RigidBodyCollider (kickerBody);
	kickerGeometry->setCollisionController (kickerCollider);

	kickerMotor = new SliderJoint  (rigidBody, kickerBody);
	kickerMotor->setAxis           (Vector3 (-1.0, 0.0, 0.0));
	kickerMotor->setMaxForce       (0.0);
	kickerMotor->setLimits         (0.0, 0.0);
	kickerMotor->setTargetVelocity (0.0);
	
	//kickerMotor->setMaxForce       (DVAR (KICKER_MAX_FORCE));
	//kickerMotor->setLimits         (DVAR (KICKER_LOWER_LIMIT), DVAR (KICKER_UPPER_LIMIT));
	//kickerMotor->setTargetVelocity (-DVAR (KICKER_TARGET_VELOCITY));

	dMass kickerMass;
	dMassSetBox    (&kickerMass, 1, DVAR (KICKER_LENGTH), DVAR (KICKER_WIDTH), DVAR (KICKER_HEIGHT));
	dMassAdjust    (&kickerMass, DVAR (KICKER_MASS));
	dBodySetMass   (kickerBody->getBody (), &kickerMass);	

	////
	
	Timer::getInstance ()->addEvent (this, new FrameEvent (), 0);
}

OmniRobot::~OmniRobot ()
{
	delete wheel1Motor;
	delete wheel2Motor;
	delete wheel3Motor;
	delete dribblerMotor;
	delete kickerMotor;
}

void OmniRobot::handleEvent (Event* event)
{
	if (!event)
	{
		return;
	}

	////

	FrameEvent* frameEvent = dynamic_cast<FrameEvent*> (event);

	if (frameEvent)
	{
/*		Matrix33 m = rigidBody->getWorldRotation () * Matrix33::rotateAboutX (Math::HALF_PI);
		
		Matrix33 worldRotation = m * Matrix33::rotateAboutY (Math::PI / 4);

		dMatrix3 r;
		r[0]  = worldRotation[0][0];
		r[1]  = worldRotation[0][1];
		r[2]  = worldRotation[0][2];
		r[3]  = 0.0;
		r[4]  = worldRotation[1][0];
		r[5]  = worldRotation[1][1];
		r[6]  = worldRotation[1][2];
		r[7]  = 0.0;
		r[8]  = worldRotation[2][0];
		r[9]  = worldRotation[2][1];
		r[10] = worldRotation[2][2];
		r[11] = 0.0;

		dBodySetRotation (wheel1Body->getBody  (), r);
		
		worldRotation = m * Matrix33::rotateAboutY (Math::HALF_PI);

		r[0]  = worldRotation[0][0];
		r[1]  = worldRotation[0][1];
		r[2]  = worldRotation[0][2];
		r[3]  = 0.0;
		r[4]  = worldRotation[1][0];
		r[5]  = worldRotation[1][1];
		r[6]  = worldRotation[1][2];
		r[7]  = 0.0;
		r[8]  = worldRotation[2][0];
		r[9]  = worldRotation[2][1];
		r[10] = worldRotation[2][2];
		r[11] = 0.0;

		dBodySetRotation (wheel2Body->getBody  (), r);
		
		worldRotation = m * Matrix33::rotateAboutY (-Math::PI / 4);
		
		r[0]  = worldRotation[0][0];
		r[1]  = worldRotation[0][1];
		r[2]  = worldRotation[0][2];
		r[3]  = 0.0;
		r[4]  = worldRotation[1][0];
		r[5]  = worldRotation[1][1];
		r[6]  = worldRotation[1][2];
		r[7]  = 0.0;
		r[8]  = worldRotation[2][0];
		r[9]  = worldRotation[2][1];
		r[10] = worldRotation[2][2];
		r[11] = 0.0;

		dBodySetRotation (wheel3Body->getBody  (), r);
*/
		Vector3 fdir = rigidBody->getWorldRotation () * Vector3::UNIT_X;
		fdir.z       = 0;
		fdir.normalize ();

		wheel1Collider->getSurface ()->setFrictionDirection (Matrix33::rotateAboutZ ( Math::PI / 6) * fdir);
		wheel2Collider->getSurface ()->setFrictionDirection (Matrix33::rotateAboutZ ( Math::PI / 2) * fdir);
		wheel3Collider->getSurface ()->setFrictionDirection (Matrix33::rotateAboutZ (-Math::PI / 6) * fdir);

/*		
		fdir = Matrix33::rotateAboutZ (5 * Math::PI / 6) * fdir;
		wheel1Collider->getSurface ()->setFrictionDirection (fdir);
		
		fdir = Matrix33::rotateAboutZ (2 * Math::PI / 3) * fdir;
		wheel2Collider->getSurface ()->setFrictionDirection (fdir);

		fdir = Matrix33::rotateAboutZ (-2 * Math::PI / 3) * fdir;
		wheel3Collider->getSurface ()->setFrictionDirection (fdir);
*/

		delete frameEvent;
		
		Timer::getInstance ()->addEvent (this, new FrameEvent (), 0.001);
		return;
	}

	////
	
	VelocityEvent* velocityEvent = dynamic_cast<VelocityEvent *> (event);

	if (velocityEvent)
	{
		targetVelocity.x = velocityEvent->linearVelocity.x;
		targetVelocity.y = velocityEvent->linearVelocity.y;
		targetVelocity.z = velocityEvent->angularVelocity.z;


		Vector3 w = transform * targetVelocity;

		wheel1Motor->setTargetVelocity (w.x);
		wheel2Motor->setTargetVelocity (w.y);
		wheel3Motor->setTargetVelocity (w.z);


		delete velocityEvent;
		return;
	}

	////

	KickerEvent* kickerEvent = dynamic_cast<KickerEvent *> (event);

	if (kickerEvent)
	{
		if (kickerEvent->isEnabled)
		{
			kickerMotor->setTargetVelocity  (DVAR (KICKER_TARGET_VELOCITY));
			kickerMotor->setMaxForce        (DVAR (KICKER_MAX_FORCE));
			kickerMotor->setLimits          (DVAR (KICKER_LOWER_LIMIT), DVAR (KICKER_UPPER_LIMIT));
			Timer::getInstance ()->addEvent (this, new KickerEvent (false), DVAR (KICKER_DELAY));
		}
		else
		{
			kickerMotor->setTargetVelocity (0.0);
			kickerMotor->setMaxForce       (0.0);
			kickerMotor->setLimits         (0.0, 0.0);
		}

		delete kickerEvent;
		return;
	}

	////

	DribblerEvent* dribblerEvent = dynamic_cast<DribblerEvent *> (event);

	if (dribblerEvent)
	{
		if (dribblerEvent->isEnabled)
		{
			dribblerMotor-> setTargetVelocity (DVAR (DRIBBLER_TARGET_VELOCITY));
		}
		else
		{
			dribblerMotor-> setTargetVelocity (0.0);
		}

		delete dribblerEvent;
		return;
	}

	////
	
	RigidBodyEntity::handleEvent (event);
}
